#pragma once

#include "common.h"

namespace TorPhysix
{
class RigidBody
{
public:
	RigidBody();
	
	// damping for linear and angular velocities, used to stabilize overshooting Euler integrator
	real linearDamping; 
	real angularDamping;
	
	// inverse mass so we don't have to divide by one on every integrate step
	real inverseMass; 
	
	// inverse inertia tensor in local coordinate space, it is inverted for the same reasons we invert mass
	Mat3f inverseInertiaTensor; 
	
	//  when we apply forces, we do so in world coordinates, keep a cached version of the inertia tensor in world/global coordinates 
	Mat3f inverseInertiaTensorWorld; 
	
	// Position is the same as the body center of mass 
	Vec3f position; 
	
	// orientation is represented with a quaternion, it could have been represented with a 3x3 matrix, but its more work to keep a matrix orthonormal
	Quatf orientation; 
	
	// linear velocity
	Vec3f velocity; 
	
	// angular velocity
	Vec3f rotation; 
	
	/** A cached transformation matrix used to convert from body space into world space and back
	  * we could have used position and orientation every time we do a transform, but keeping a cached matrix of both is convenient
    */
	Mat4f transformMatrix;
	
	// Accumulator for linear forces
	Vec3f forceAccum;
	
	// Accumulator for torques
	Vec3f torqueAccum;
	
	// Acceleration due to a constant force like gravity, saves us setting it every frame
	Vec3f acceleration;
	
	// A variable that stores the acceleration last frame, 
	// a "hack" used to prevent a resting object from sinking into the ground and bouncing up next frame
	Vec3f lastFrameAcceleration;

	// Can this object be moved by solving contacts?
	bool isStatic;
	
	// Calculates cached transformation matrix and inverse inertia tensor in world coordinates
	void calculateDerivedData(); 

	// Integrate linear and angular forces and apply new velocity and position
	void integrate(real dt);

	 // Clear force and torque vectors
	void clearAccumulators();

	// Takes an non-inverted inertia tensor
	void setInertiaTensor( const Mat3f &inertiaTensor ); 
	void setAcceleration(const Vec3f &accel){ acceleration = accel; }
	void setVelocity( const Vec3f &vel ){ velocity = vel; }
	void setRotation( const Vec3f &angularVelocity ){ rotation = angularVelocity; }
	void setOrientation(const Quatf orient);
	void setPosition( const Vec3f &pos ){ position = pos; }
	void setPosition( real x, real y, real z );
	void setDamping(const real linear, const real angular){ linearDamping = linear; angularDamping = angular; }
	void setMass(const real mass){ inverseMass = real(1.0) / mass; }
	
	void addVelocity(const Vec3f &deltaVelocity){ velocity += deltaVelocity; }
	void addRotation(const Vec3f &deltaRotation){ rotation += deltaRotation; }
	void addForce(const Vec3f &force){ forceAccum += force; }
	void addTorque(const Vec3f &torque){ torqueAccum += torque; }

	// Add a force in world coordinates. Useful for spring forces or other forces fixed to the body
	// @param force The force to apply
	// @param point The location in local/body coordinates
	void addForceAtBodyPoint( const Vec3f &force, const Vec3f &point );

	// Add a force at a point, both forces are expected to be in world coordinates
	// because the force isn't applied to the center of mass, it may generate torque
	// @param force Force in world coordinates
	// @param point point in world coordinates
	void addForceAtPoint( const Vec3f &force, const Vec3f &point );


	// Transforms world space point to body space point, removes rotations and translations the body may have undergone and gives us
	// the point relative to the body frame of reference 
	Vec3f getPointInLocalSpace( const Vec3f &point ); 

	// Transforms the point relative to world coordinates
	Vec3f getPointInWorldSpace( const Vec3f &point ); 

	// Converts body position and orientation to one that can be loaded into OpenGL with glMultMatrixf
	void getGLTransform(float matrix[16]) const; 
	Mat4f getTransform() const{ return transformMatrix; }
	real getInverseMass() const { return inverseMass; }
	Vec3f getPosition() const { return position; }
	void getOrientation(Quatf *orientation) const{ *orientation = RigidBody::orientation; }
	Quatf getOrientation() const{ return RigidBody::orientation; }
	Vec3f getVelocity() const { return velocity; }
	Vec3f getRotation() const { return rotation; }
	Vec3f getLastFrameAcceleration() const { return lastFrameAcceleration; } 
	Mat3f getInverseInertiaTensorWorld( ) const { return inverseInertiaTensorWorld; }
	void getInverseInertiaTensorWorld(Mat3f *inverseInertiaTensor) const{ *inverseInertiaTensor = inverseInertiaTensorWorld; }
	void setStatic( bool isStatic );

private:
	
};

} // end namespace TorPhysix